#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/simple_json.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#include <math.h>

#define TAG "Main"

float delta_seconds = 0;

void get_delta_seconds()
{
  static uint32_t last_ms = 0;
  uint32_t cur_ms;
  mspm0_get_clock_ms(&cur_ms);

  delta_seconds = (cur_ms - last_ms) / 1000.0f;
  last_ms = cur_ms;
}

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motion_init();
  OLED_ShowString(0, 0, "Motor PID ", 16);

  float left_speed = 0;
  float right_speed = 0;

  uint32_t i = 0;

  int16_t speed = 100;
  bool flag = 1;

  // motor_set_mode(MOTOR_REAR | MOTOR_LEFT, MOTOR_FORWARD);
  // motor_set_mode(MOTOR_REAR | MOTOR_REAR, MOTOR_FORWARD);
  // motor_set_torque(MOTOR_REAR | MOTOR_LEFT, speed);
  // motor_set_torque(MOTOR_REAR | MOTOR_RIGHT, speed);

  while (1)
  {
    get_delta_seconds();
    motion_move_pool(delta_seconds);

#if 1
    right_speed = motor_get_speed(MOTOR_REAR | MOTOR_RIGHT);
    left_speed = motor_get_speed(MOTOR_REAR | MOTOR_LEFT);
    my_printf("<%d>{speed}%f,%f,%d\n", i++, left_speed, right_speed, speed);
#endif

    if (flag)
    {
      speed--;
      if (speed <= 0)
        flag = false;
    }
    else
    {
      speed++;
      if (speed >= 100)
        flag = true;
    }
    motion_set_speed(speed);

    mspm0_delay_ms(10);
  }
}
